#include "bsp.h"

volatile tMPU6050_Data MPU6050_Data;

#define MPU6050_ADDRESS 0xD0

void MyI2C_W_SCL(uint8_t BitValue)
{
    HAL_GPIO_WritePin(GPIOB, MyI2C_CLK_Pin, (GPIO_PinState)BitValue);
    delay_us(10);
}

void MyI2C_W_SDA(uint8_t BitValue)
{
    HAL_GPIO_WritePin(GPIOB, MyI2C_SDA_Pin, (GPIO_PinState)BitValue);
    delay_us(10);
}

uint8_t MyI2C_R_SDA(void)
{
    uint8_t BitValue;
    BitValue = HAL_GPIO_ReadPin(GPIOB, MyI2C_SDA_Pin);
    delay_us(10);
    return BitValue;
}

void MyI2C_Start(void)
{
    MyI2C_W_SDA(1);
    MyI2C_W_SCL(1);
    MyI2C_W_SDA(0);
    MyI2C_W_SCL(0);
}

void MyI2C_Stop(void)
{
    MyI2C_W_SDA(0);
    MyI2C_W_SCL(1);
    MyI2C_W_SDA(1);
}

void MyI2C_SendByte(uint8_t Byte)
{
    uint8_t i;

    for (i = 0; i < 8 ; i++) {
        MyI2C_W_SDA(Byte & (0x80 >> i));
        MyI2C_W_SCL(1);
        MyI2C_W_SCL(0);
    }
}

uint8_t MyI2C_ReceiveByte(void)
{
    uint8_t i, Byte = 0x00;
    MyI2C_W_SDA(1);

    for (i = 0; i < 8; i++) {
        MyI2C_W_SCL(1);

        if (MyI2C_R_SDA() == 1) {
            Byte |= (0x80 >> i);
        }

        MyI2C_W_SCL(0);
    }

    return Byte;
}

void MyI2C_SendAck(uint8_t AckBit)
{
    MyI2C_W_SDA(AckBit);
    MyI2C_W_SCL(1);
    MyI2C_W_SCL(0);
}

uint8_t MyI2C_ReceiveAck(void)
{
    uint8_t AckBit;
    MyI2C_W_SDA(1);
    MyI2C_W_SCL(1);
    AckBit = MyI2C_R_SDA();
    MyI2C_W_SCL(0);
    return AckBit;
}

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
		MyI2C_Start();
		MyI2C_SendByte(MPU6050_ADDRESS);
		MyI2C_ReceiveAck();
		MyI2C_SendByte(RegAddress);
		MyI2C_ReceiveAck();
		MyI2C_SendByte(Data);
		MyI2C_ReceiveAck();	
		MyI2C_Stop();
}

uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
		uint8_t Data;
	
		MyI2C_Start();
		MyI2C_SendByte(MPU6050_ADDRESS);
		MyI2C_ReceiveAck();
		MyI2C_SendByte(RegAddress);
		MyI2C_ReceiveAck();
	
		MyI2C_Start();
		MyI2C_SendByte(MPU6050_ADDRESS | 0x01);
		MyI2C_ReceiveAck();
		Data = MyI2C_ReceiveByte();
		MyI2C_SendAck(1);
		MyI2C_Stop();
	
		return Data;
}

uint8_t  MPU6050_Read_ID(void)
{
		uint8_t ID = MPU6050_ReadReg(MPU6050_WHO_AM_I);
//		OLED_ShowHexNum(1,1,ID,2);
	return ID;
}


void MPU6050_Init(void)
{
		MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
		MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
		MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
		MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
		MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
		MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}

void MPU6050_GetData1(void)
{
	uint8_t DataH, DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
	MPU6050_Data.AccX = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
	MPU6050_Data.AccY = (DataH << 8) | DataL;

	DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
	MPU6050_Data.AccZ = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
	MPU6050_Data.GyroX = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
	MPU6050_Data.GyroY = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
	MPU6050_Data.GyroZ = (DataH << 8) | DataL;
}
